#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>

int kfd = 0;
struct termios cooked, raw;

void init_keyboard()
{
    // 获取标准输入的文件描述符
    tcgetattr(kfd, &cooked);                       // 获取标准输入的属性
    memcpy(&raw, &cooked, sizeof(struct termios)); // 复制一份cooked的配置
    raw.c_lflag &= ~(ICANON | ECHO);               // 关闭标准输入的标准模式，使输入字符不需要回车确认
    raw.c_cc[VEOL] = 1;                            // 有些系统下不设置会出现字符打印问题
    tcsetattr(kfd, TCSANOW, &raw);                 // 设置属性
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "keyboard_control");
    ros::NodeHandle nh;

    // 创建一个发布者，用于发布速度指令
    ros::Publisher vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

    // 初始化键盘
    init_keyboard();

    // 创建一个速度消息
    geometry_msgs::Twist vel_msg;

    // 控制机器人运动
    while (ros::ok())
    {
        char c;
        if (read(kfd, &c, 1) < 0) // 读取键盘输入
        {
            perror("read():");
            exit(-1);
        }

        switch (c)
        {
        case 'w':
            vel_msg.linear.x += 0.8;
            vel_msg.angular.z += 0;
            break;

        case 's':
            vel_msg.linear.x += -0.8;
            vel_msg.angular.z += 0;
            break;
        case 'a':
            vel_msg.linear.x += 0.0;
            vel_msg.angular.z += 0.8;
            break;

        case 'd':
            vel_msg.linear.x += 0.0;
            vel_msg.angular.z += -0.8;
            break;

        default:
            vel_msg.linear.x = 0.0;
            vel_msg.angular.z = 0.0;
        }

        // 发布速度消息
        vel_pub.publish(vel_msg);
        ros::spinOnce();
    }

    // 恢复标准输入属性
    tcsetattr(kfd, TCSANOW, &cooked);

    return 0;
}
